#include "NXTdc.h"

#define max_message_size 59

int ResetSensor(char port){
	char portReset[5] = {0x03, 0x00, 0x00, 0x08, port};
	char replyReset[max_message_size];
	
	write(nxtsocket, portReset, 5);
	read(nxtsocket, replyReset, 5);
	return replyReset[4];
}

int SetSensorType(char port, char type){
	char portSet[7] = {0x05, 0x00, 0x00, 0x05, port, type, 0x80};
	char replySet[max_message_size];
	
	write(nxtsocket, portSet, 7);
	read(nxtsocket, replySet, 5);
	return replySet[4];
}

int SetSensorLight(char port){
	char portSet[7] = {0x05, 0x00, 0x00, 0x05, port, 0x05, 0x80};
	char replySet[max_message_size];
	
	write(nxtsocket, portSet, 7);
	read(nxtsocket, replySet, 5);
	return replySet[4];
}

int SetSensorSound(char port){
	char portSet[7] = {0x05, 0x00, 0x00, 0x05, port, 0x07, 0x80};
	char replySet[max_message_size];
	
	write(nxtsocket, portSet, 7);
	read(nxtsocket, replySet, 5);
	return replySet[4];
}

int Sensor(char port){
	char portGet[5] = {0x03, 0x00, 0x00, 0x07, port};
	char replyGet[max_message_size];

	write(nxtsocket, portGet, 5);
	read(nxtsocket, replyGet, 18);

	if (replyGet[4] == 0) return replyGet[14];
	else return replyGet[4];
}

int SetSensorLowspeed(char port){
	char portSet[7] = {0x05, 0x00, 0x00, 0x05, port, 0x0B, 0x00};
	char replySet[max_message_size];

	write(nxtsocket, portSet, 7);
	read(nxtsocket, replySet, 5);
	return replySet[4];
}

int SensorUS(char port){
	char portGet[9] = {0x07, 0x00, 0x00, 0x0F, port, 0x02, 0x01, 0x02, 0x42};
	char portReady[5] = {0x03, 0x00, 0x00, 0x0E, port};
	char portRead[5] = {0x03, 0x00, 0x00, 0x10, port};
	char replyGet[max_message_size];
	char replyReady[max_message_size];
	char replyRead[max_message_size];

	write(nxtsocket, portGet, 9);
	read(nxtsocket, replyGet, 5);
	if (replyGet[4] != 0) return replyGet[4];

	do {
		write(nxtsocket, portReady, 5);
		read(nxtsocket, replyReady, 6);
	}
	while (!((replyReady[2] == 0x02)&&(replyReady[3] == 0x0E)&&(replyReady[4] == 0x00)&&(replyReady[5] == 0x01)));
	
	write(nxtsocket, portRead, 5);
	read(nxtsocket, replyRead, 22);
	if (replyRead[4] == 0) return replyRead[6];
	else return replyRead[4];
}

int PlayTone(unsigned int frequency, unsigned int duration){
	char cmd[8] = {0x06, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00};
	char replyCmd[max_message_size];
	cmd[4] = frequency        & 0xFF;
	cmd[5] = (frequency >> 8) & 0xFF;
	cmd[6] = duration         & 0xFF;
	cmd[7] = (duration >> 8)  & 0xFF;
	
	write(nxtsocket, cmd, 8);
	read(nxtsocket, replyCmd, 5);
	return replyCmd[4];
}


int OnFwd(char port, char power){
	char cmd[14] = {0x0C, 0x00, 0x00, 0x04, port, power, 0x05, 0x01, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00};
	char replyCmd[max_message_size];

	write(nxtsocket, cmd, 14);
	read(nxtsocket, replyCmd, 5);

	return replyCmd[4];
}

int OnRev(char port, char power){
	char cmd[14] = {0x0C, 0x00, 0x00, 0x04, port, 0x00, 0x05, 0x01, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00};
	char replyCmd[max_message_size];

	cmd[5] = power ^ 0xFF;
	write(nxtsocket, cmd, 14);
	read(nxtsocket, replyCmd, 5);

	return replyCmd[4];
}


int Off(char port){
	char cmd[14] = {0x0C, 0x00, 0x00, 0x04, port, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
	char replyCmd[max_message_size];

	write(nxtsocket, cmd, 14);
	read(nxtsocket, replyCmd, 5);

	return replyCmd[4];
}

int RotateMotor(char port, char power, long angle){
	char cmd[14] = {0x0C, 0x00, 0x00, 0x04, port, power, 0x05, 0x02, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00};
	char replyCmd[max_message_size];

	if (angle != 0.0){
		cmd[10] = angle         & 0xFF;
		cmd[11] = (angle >> 8)  & 0xFF;
		cmd[12] = (angle >> 16) & 0xFF;
		cmd[13] = (angle >> 24) & 0xFF;
	}

	write(nxtsocket, cmd, 14);
	read(nxtsocket, replyCmd, 5);

	return replyCmd[4];
}

int RotateMotorEx(char port, char power, long angle, char turnpct, int sync, int stop){
	char cmd[14] = {0x0C, 0x00, 0x00, 0x04, port, power, 0x05, 0x02, turnpct, 0x10, 0x00, 0x00, 0x00, 0x00};
	char replyCmd[max_message_size];

	if (stop) {
		cmd[6] = 0x70;
		cmd[9] = 0x40;
	}

	if ((angle != 0.0)&&(sync)){
		cmd[10] = angle         & 0xFF;
		cmd[11] = (angle >> 8)  & 0xFF;
		cmd[12] = (angle >> 16) & 0xFF;
		cmd[13] = (angle >> 24) & 0xFF;
	}

	write(nxtsocket, cmd, 14);
	read(nxtsocket, replyCmd, 5);

	return replyCmd[4];
}

int ResetRotationCount(char port)
{
	char portReset[6] = {0x04, 0x00, 0x00, 0x0A, port, 0x01};
	char replyReset[max_message_size];

	write(nxtsocket, portReset, 6);
	read(nxtsocket, replyReset, 5);

	return replyReset[4];
}

int ResetTachoCount(char port)
{
	char portReset[6] = {0x04, 0x00, 0x00, 0x0A, port, 0x00};
	char replyReset[max_message_size];

	write(nxtsocket, portReset, 6);
	read(nxtsocket, replyReset, 5);

	return replyReset[4];
}

long MotorRotationCount(char port){
	char portGet[5] = {0x03, 0x00, 0x00, 0x06, port};
	char replyGet[max_message_size];

	write(nxtsocket, portGet, 5);

	read(nxtsocket, replyGet, 27);
	if (replyGet[4] == 0) return (long) (replyGet[23] | (replyGet[24] << 8) | (replyGet[25] << 16) | (replyGet[26] << 24));
	else return (long) replyGet[4];
}

long MotorTachoCount(char port){
	char portGet[5] = {0x03, 0x00, 0x00, 0x06, port};
	char replyGet[max_message_size];

	write(nxtsocket, portGet, 5);

	read(nxtsocket, replyGet, 27);
	if (replyGet[4] == 0) return (long) (replyGet[15] | (replyGet[16] << 8) | (replyGet[17] << 16) | (replyGet[18] << 24));
	else return (long) replyGet[4];
}
